#include <Servo.h>
// TODO: Have the arm move at random angular dist or speed?
// #include <stdlib.h>

Servo myservo;
Servo lidservo;


const int HIDDEN_POS = 125;
const int VISIBLE_POS = 0;
const int HIDDEN_POS_LID = 0;
const int VISIBLE_POS_LID = 175; 

int count = 0;
int digitalPin = 2;
int servoPin = 5;
int servoPin2 = 6;

void setup() {
  count = 0;
  // put your setup code here, to run once:
  myservo.attach(servoPin);
  lidservo.attach(servoPin2);
  // mainly for debugging
  Serial.begin(9600);

  // sets up switch and servo motor 
  pinMode(digitalPin, INPUT_PULLUP);
  myservo.write(HIDDEN_POS);
  lidservo.write(HIDDEN_POS_LID);
}

void loop() {
  //Serial.println(digitalRead(digitalPin));
  //delay(1);        // delay in between reads for stabilitydff
  if(count > 8) {
    count = 0;
  }
  if (digitalRead(digitalPin) == LOW) {
    lidservo.write(VISIBLE_POS_LID);
    delay(200);
    //done to produce some variation in the machine's behavior: will move the arm up a variable distance before pushing the switch
    //Servo goes from 135(HIDDEN_POS) to a number between 0 and 40
    myservo.write(VISIBLE_POS + count * 10);
    
    //delay of variable length to show the variable movement
    delay(500 * count);

    // random movement
    if (count % 2 == 0) {
      myservo.write(HIDDEN_POS - count * 5);
      delay(250 * count);
    }
    //without this the switch is never pushed and the program gets stuck
    myservo.write(VISIBLE_POS);
    count++;
    while(digitalRead(digitalPin) == LOW){
      Serial.println(digitalRead(digitalPin));
     }
     
    
    myservo.write(HIDDEN_POS);
    // note: The lid coming down has knocked the plush, servo and arduino out of alignment, so I've tried making it slower. 
    // I haven't had much success. 
    lidservo.write(HIDDEN_POS_LID + 100);
    delay(2000 * 8 - count);
    lidservo.write(HIDDEN_POS_LID + 50);
    delay(1000);
    lidservo.write(HIDDEN_POS_LID);
    Serial.println(digitalRead(digitalPin));

  }


}
